/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *                                                                           *
 *                          Copyright (c) 2010                               *
 *                  Alen Stojanov [ ioi.alen@gmail.com ]                     *
 *                Jacobs University Bremen, Bremen, Germany                  *
 *                                                                           *
 *                          All Rights Reserved.                             *
 *                                                                           *
 *    Permission to use, copy, modify and distribute this software and       *
 *    its documentation is hereby granted, provided that both the            *
 *    copyright notice and this permission notice appear in all copies of    *
 *    the software, derivative works or modified versions, and any           *
 *    portions thereof, and that both notices appear in supporting           *
 *    documentation.                                                         *
 *                                                                           *
 *    This code is distributed in the hope that it will be useful, but       *
 *    WITHOUT ANY WARRANTY; without even the implied warranty of             *
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. THE AUTHORS       *
 *    DISCLAIM ANY LIABILITY OF ANY KIND FOR ANY DAMAGES WHATSOEVER          *
 *    RESULTING FROM THE USE OF THIS SOFTWARE.                               *
 *                                                                           *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include "grahamscan.h"

bool GSTSPointCompare::operator()(const GSTSPoint& p1, const GSTSPoint& p2)
{
	if (p1.angle < p2.angle) {
		return true;
	} else if (p1.angle > p2.angle) {
		return false;
	} else if (is_equal(p1,p2)) {
		return false;
	} else if (lay_distance(anchor->x, anchor->y, p1.x, p1.y) <
			lay_distance(anchor->x, anchor->y, p2.x, p2.y)) {
		return true;
	} else {
		return false;
	}
}

bool GSTSPointCompare::is_equal(const GSTSPoint p1, GSTSPoint p2)
{
	return  is_equal(p1.x,p2.x) && is_equal(p1.y,p2.y);
}

bool GSTSPointCompare::is_equal(const double v1, const double& v2,
		const double epsilon)
{
	double diff = v1 - v2;
	return  (-epsilon <= diff) && (diff <= epsilon);
}

double GSTSPointCompare::lay_distance(const double& x1,
		const double& y1, const double& x2, const double& y2)
{
	double dx = (x1 - x2);
	double dy = (y1 - y2);
	return (dx * dx + dy * dy);
}

bool GrahamScan::operator()(const QVector <TSPoint>& pnt,
		QVector<TSPoint>& final_hull)
{
	final_hull.clear();

	if (pnt.size() <= 3) {
		std::copy(pnt.begin(), pnt.end(), std::back_inserter(final_hull));
		return true;
	}

	unsigned int j = 0;
	GSTSPoint tmp_pnt;

	for(int i = 0; i < pnt.size(); i++) {
		point.push_back(GSTSPoint(pnt[i].x,pnt[i].y,0.0));
		if (point[i].y < point[j].y) {
			j = i;
		} else if (point[i].y == point[j].y) {
			if (point[i].x < point[j].x) {
				j = i;
			}
		}
	}

	tmp_pnt  = point[0];
	point[0] = point[j];
	point[j] = tmp_pnt;

	anchor = point[0];

	for (unsigned int i = 1; i < point.size(); i++) {
		point[i].angle = cartesian_angle(point[i].x - anchor.x, point[i].y - anchor.y);
	}

	sort(++point.begin(),point.end(),GSTSPointCompare(&anchor));
	graham_scan(final_hull);

	return true;
}

void GrahamScan::graham_scan(QVector< TSPoint >& final_hull)
{
	const std::size_t HEAD     = 0;
	const std::size_t PRE_HEAD = 1;

	QQueue<GSTSPoint> pnt_queue;

	pnt_queue.push_front(point[0]);
	pnt_queue.push_front(point[1]);

	unsigned int i = 2;

	while(i < point.size()) {
		if (pnt_queue.size() > 1) {
			if (orientation(pnt_queue[PRE_HEAD],pnt_queue[HEAD],point[i]) == counter_clock_wise) {
				pnt_queue.push_front(point[i++]);
			} else {
				pnt_queue.pop_front();
			}
		}
		else {
			pnt_queue.push_front(point[i++]);
		}
	}

	for(QQueue<GSTSPoint>::iterator it = pnt_queue.begin(); it != pnt_queue.end(); it++) {
		final_hull.push_back(TSPoint((*it).x, (*it).y));
	}
}

inline double GrahamScan::cartesian_angle(double x, double y)
{
	if		((x >  0.0) && (y >  0.0)) return (atan( y / x) * _180DivPI);
	else if	((x <  0.0) && (y >  0.0)) return (atan(-x / y) * _180DivPI) +  90.0;
	else if	((x <  0.0) && (y <  0.0)) return (atan( y / x) * _180DivPI) + 180.0;
	else if	((x >  0.0) && (y <  0.0)) return (atan(-x / y) * _180DivPI) + 270.0;
	else if	((x == 0.0) && (y >  0.0)) return  90.0;
	else if	((x <  0.0) && (y == 0.0)) return 180.0;
	else if	((x == 0.0) && (y <  0.0)) return 270.0;
	else return 0.0;
}

inline int GrahamScan::orientation(const GSTSPoint& p1,
		const GSTSPoint& p2, const GSTSPoint& p3)
{
	return orientation(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y);
}

inline int GrahamScan::orientation(
		const double x1, const double y1,
		const double x2, const double y2,
		const double px, const double py)
{
	double orin = (x2 - x1) * (py - y1) - (px - x1) * (y2 - y1);

	if (is_equal(orin,0.0)) {
		return 0;              /* Orientaion is neutral aka collinear  */
	} else if (orin < 0.0) {
		return -1;             /* Orientaion is to the right-hand side */
	} else {
		return +1;             /* Orientaion is to the left-hand side  */
	}
}

inline bool GrahamScan::is_equal(const double v1,
		const double& v2, const double epsilon)
{
	double diff = v1 - v2;
	return  (-epsilon <= diff) && (diff <= epsilon);
}
